/******************************************************************************
*  Nano-RK, a real-time operating system for sensor networks.
*  Copyright (C) 2007, Real-Time and Multimedia Lab, Carnegie Mellon University
*  All rights reserved.
*
*  This is the Open Source Version of Nano-RK included as part of a Dual
*  Licensing Model. If you are unsure which license to use please refer to:
*  http://www.nanork.org/nano-RK/wiki/Licensing
*
*  This program is free software: you can redistribute it and/or modify
*  it under the terms of the GNU General Public License as published by
*  the Free Software Foundation, version 2.0 of the License.
*
*  This program is distributed in the hope that it will be useful,
*  but WITHOUT ANY WARRANTY; without even the implied warranty of
*  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
*  GNU General Public License for more details.
*
*  You should have received a copy of the GNU General Public License
*  along with this program.  If not, see <http://www.gnu.org/licenses/>.
*
*  Contributing Authors (specific to this file):
*  Anthony Rowe
*  Zane Starr
*  Anand Eswaren
*******************************************************************************/

#include <nrk.h>
#include <nrk_task.h>
#include <include.h>
#include <ulib.h>
#include <nrk_timer.h>
#include <nrk_scheduler.h>
#include <nrk_error.h>
#include <nrk_stack_check.h>
#include <nrk_defs.h>

//#define TIME_PAD  2

inline void _nrk_wait_for_scheduler ();

uint8_t nrk_get_high_ready_task_ID ()
{
  return (_head_node->task_ID);
}

void nrk_print_readyQ ()
{
  nrk_queue *ptr;
  ptr = _head_node;
  //nrk_kprintf (PSTR ("nrk_queue: "));
  while (ptr != NULL) {
    //printf ("%d ", ptr->task_ID);
    ptr = ptr->Next;
  }
  //nrk_kprintf (PSTR ("\n\r"));
}

void nrk_add_to_readyQ (int8_t task_ID)
{
	nrk_queue *NextNode;
	nrk_queue *CurNode;

	//printf( "nrk_add_to_readyQ %d\n",task_ID );
	
	// nrk_queue full
	if (_free_node == NULL)
	{
		return;
	}
	
	// Setup for looping
	NextNode = _head_node;
	CurNode = _free_node;
	
	// If ready queue is not empty
	if (_head_node != NULL)
	{
		/* SRP */
		// Everything in the ready queue had a arrival time earlier than the one that
		// is currently being added.  Just check preemption level.
		
		// Calculate system ceiling from currently locked resources
		// System ceiling = highest resource ceiling among all currently locked resources		
		uint8_t system_ceiling = 0;				//sys_ceil_stk_peek();
		// Loop through resources to get maximum resource ceiling
		for(int i = 0; i < NRK_MAX_RESOURCE_CNT; i++)
		{
			// Only consider for system ceiling if currently locked
			if(nrk_sem_query(&nrk_sem_list[i]) == 0)
			{
				int8_t resource_ceiling = max_period - nrk_sem_list[i].resource_ceiling;
				if(resource_ceiling > system_ceiling)
				{
					system_ceiling = resource_ceiling;
				}
			}
		}
		
	          
		uint16_t preemption_level = max_period - nrk_task_TCB[task_ID].period;
		
		// If preemption level > system ceiling, stick this job at the front of the ready queue
		// Else we do the loopy thing to put it where it should be.
		if(preemption_level <= system_ceiling)
		{
			// Loop through all tasks in ready queue
			while (NextNode != NULL)
			{		
				/* EDF */
				// Looping through tasks of smallest next_period to largest next_period
				// Find the place where our next_period is still smaller than the next node's next_period
				/* break out if end of task list or at a closer deadline */
				if (NextNode->task_ID == NRK_IDLE_TASK_ID ||
					(nrk_task_TCB[NextNode->task_ID].next_period > nrk_task_TCB[task_ID].next_period) ||
                    (nrk_task_TCB[NextNode->task_ID].next_period == nrk_task_TCB[task_ID].next_period &&
                     nrk_task_TCB[NextNode->task_ID].task_prio > nrk_task_TCB[task_ID].task_prio))
				break;
				/* --------------- */
				/* static priority */
				/*
				  if (nrk_task_TCB[NextNode->task_ID].elevated_prio_flag)
					if (nrk_task_TCB[NextNode->task_ID].task_prio_ceil <
						nrk_task_TCB[task_ID].task_prio)
					  break;
				  if (nrk_task_TCB[task_ID].elevated_prio_flag)
					if (nrk_task_TCB[NextNode->task_ID].task_prio <
						nrk_task_TCB[task_ID].task_prio_ceil)
					  break;
				  if (nrk_task_TCB[NextNode->task_ID].task_prio <
					  nrk_task_TCB[task_ID].task_prio)
					break;
				*/
				
				// Keep going
				NextNode = NextNode->Next;
			}
		}
		/* End SRP */

		//      while ((NextNode != NULL) && ((nrk_task_TCB[NextNode->task_ID].task_prio >= nrk_task_TCB[task_ID].task_prio)|| ) {
		//              NextNode = NextNode->Next;}
		// Stop if nextNode is freenode or next node less prio or (equal and elevated 
		// Issues - 1 comes, becomes 2', 1 more comes (2' 1) then 2 comes where should it be placed ?
		// 2' 2  1 or 2 2' 1 in ready q , what happens after 2'->1, what if 2'->2 
	}

	// Take a "free node" from the free node chain and allocate to the task we're adding
	CurNode->task_ID = task_ID;
	_free_node = _free_node->Next; // _free_node is the node after the last node in the linked list

	if (NextNode == _head_node)
	{
		//at start
		if (_head_node != NULL)
		{
			CurNode->Next = _head_node;
			CurNode->Prev = NULL;
			_head_node->Prev = CurNode;
		}
		else
		{
			CurNode->Next = NULL;
			CurNode->Prev = NULL;
			_free_node->Prev = CurNode;
		}
		_head_node = CurNode;

	}
	else
	{
		if (NextNode != _free_node)
		{
			// Insert  in middle
			CurNode->Prev = NextNode->Prev;
			CurNode->Next = NextNode;
			(NextNode->Prev)->Next = CurNode;
			NextNode->Prev = CurNode;
		}
		else
		{
			//insert at end
			CurNode->Next = NULL;
			CurNode->Prev = _free_node->Prev;
			_free_node->Prev = CurNode;
		}
	}
}


void nrk_rem_from_readyQ (int8_t task_ID)
{
	nrk_queue *CurNode;

	// Nothing in the queue, so return nothing
	if(_head_node == NULL)
		return;

	CurNode = _head_node;

	// Found the task at the head of the list
	if(_head_node->task_ID == task_ID)
	{
		//Remove from start
		_head_node = _head_node->Next;
		_head_node->Prev = NULL;
	}
	else
	{
		// Loop through the rest of the linked list to find the task from ready queue
		while((CurNode != NULL) && (CurNode->task_ID != task_ID))
			CurNode = CurNode->Next;
		
		// Did not find the task ID in the queue, return nothing.
		if(CurNode == NULL)
			return;

		// Remove the found task from the queue, don't mess up the pointers
		(CurNode->Prev)->Next = CurNode->Next;      //Both for middle and end
		if (CurNode->Next != NULL)
			(CurNode->Next)->Prev = CurNode->Prev;    // Only for middle
	}



	// Add to free list
	if(_free_node == NULL)
	{
		_free_node = CurNode;
		_free_node->Next = NULL;
	}
	else
	{
		CurNode->Next = _free_node;
		_free_node->Prev = CurNode;
		_free_node = CurNode;
	}
	_free_node->Prev = NULL;
}





nrk_status_t nrk_activate_task (nrk_task_type * Task)
{
  uint8_t rtype;
  void *topOfStackPtr;

  topOfStackPtr =
    (void *) nrk_task_stk_init (Task->task, Task->Ptos, Task->Pbos);

  //printf("activate %d\n",(int)Task.task_ID);
  if (Task->FirstActivation == TRUE) {
    rtype = nrk_TCB_init (Task, topOfStackPtr, Task->Pbos, 0, (void *) 0, 0);
    Task->FirstActivation = FALSE;

  }
  else {
    if (nrk_task_TCB[Task->task_ID].task_state != SUSPENDED)
      return NRK_ERROR;
    //Re-init some parts of TCB

    nrk_task_TCB[Task->task_ID].OSTaskStkPtr = (NRK_STK *) topOfStackPtr;

  }

  //nrk_task_TCB[Task->task_ID].task_state = READY;       

  // Remove from suspended or waiting if extended

  // OSSchedLock();


  // If Idle Task then Add to ready Q
  //if(Task->task_ID==0) nrk_add_to_readyQ(Task->task_ID);
  //nrk_add_to_readyQ(Task->task_ID);
  //printf( "task %d nw %d \r\n",Task->task_ID,nrk_task_TCB[Task->task_ID].next_wakeup);
  //printf( "task %d nw %d \r\n",Task->task_ID,Task->offset.secs);
  if (nrk_task_TCB[Task->task_ID].next_wakeup == 0) {
    nrk_task_TCB[Task->task_ID].task_state = READY;
    nrk_add_to_readyQ (Task->task_ID);
  }
  
	/* SRP */
	// Figure out the resource ceilings
	// Loop through the resources this task uses
	for(int i = 0; i < Task->sem_count; i++)
	{
		nrk_sem_t *cur_sem = Task->sem_list[i];
		
		// If the task period is lower than the saved period
		// (i.e., this task's preemption level > saved preemption level)
		// overwrite it.
		if(nrk_task_TCB[Task->task_ID].period < cur_sem->resource_ceiling)
		{
			cur_sem->resource_ceiling = nrk_task_TCB[Task->task_ID].period;
		}
	}
	
	// If the period here is greater than the system max period, replace system max period
	// This is used later on to calculate the preemption level by max_period - task_period
	if(nrk_task_TCB[Task->task_ID].period > max_period)
	{
		max_period = nrk_task_TCB[Task->task_ID].period;
	}
	/* End SRP */

  return NRK_OK;
}



nrk_status_t nrk_terminate_task ()
{
  nrk_rem_from_readyQ (nrk_cur_task_TCB->task_ID);
  nrk_cur_task_TCB->task_state = FINISHED;

  // HAHA, there is NO next period...
  nrk_wait_until_next_period ();
  return NRK_OK;
}

int8_t nrk_wait_until_next_period ()
{
  uint8_t timer;

  nrk_stack_check ();
// Next Period Wakeup Time is Set inside scheduler when a task becomes Runnable 
  nrk_int_disable ();
  nrk_cur_task_TCB->num_periods = 1;
  nrk_cur_task_TCB->suspend_flag = 1;
  timer = _nrk_os_timer_get ();

//nrk_cur_task_TCB->cpu_remaining=_nrk_prev_timer_val+1;

  if (timer < (MAX_SCHED_WAKEUP_TIME - TIME_PAD))
    if ((timer + TIME_PAD) <= _nrk_get_next_wakeup ()) {
      timer += TIME_PAD;
      _nrk_prev_timer_val = timer;
      _nrk_set_next_wakeup (timer);
    }

  nrk_int_enable ();
  _nrk_wait_for_scheduler ();
  return NRK_OK;
}

int8_t nrk_wait_until_next_n_periods (uint16_t p)
{
  uint8_t timer;

  nrk_stack_check ();

  if (p == 0)
    p = 1;
// Next Period Wakeup Time is Set inside scheduler when a task becomes Runnable 
  nrk_int_disable ();
  nrk_cur_task_TCB->suspend_flag = 1;
  nrk_cur_task_TCB->num_periods = p;
  timer = _nrk_os_timer_get ();

//nrk_cur_task_TCB->cpu_remaining=_nrk_prev_timer_val+1;

// +2 allows for potential time conflict resolution
  if (timer < (MAX_SCHED_WAKEUP_TIME - TIME_PAD))       // 254 8bit overflow point - 2
    if ((timer + TIME_PAD) <= _nrk_get_next_wakeup ()) {
      timer += TIME_PAD;
      _nrk_prev_timer_val = timer;
      _nrk_set_next_wakeup (timer);
    }

  nrk_int_enable ();
  _nrk_wait_for_scheduler ();
  return NRK_OK;
}

/*
 * nrk_wait_ticks()
 *
 * This function will wait until a specified number of
 * timer ticks starting from when the task was swapped in.
 * This means that this function can set periodic timing
 * taking into account any task processing time.
 *
 */

int8_t nrk_wait_ticks (uint16_t ticks)
{
  uint8_t timer;
  nrk_int_disable ();
  nrk_cur_task_TCB->suspend_flag = 1;
  timer = _nrk_os_timer_get ();
  nrk_cur_task_TCB->next_wakeup = ticks + timer;

  if (timer < MAX_SCHED_WAKEUP_TIME - TIME_PAD)
    if ((timer + TIME_PAD) <= _nrk_get_next_wakeup ()) {
      timer += TIME_PAD;
      _nrk_prev_timer_val = timer;
      _nrk_set_next_wakeup (timer);
    }
//else nrk_cur_task_TCB->next_wakeup=ticks+1;
  nrk_int_enable ();
//while(nrk_cur_task_TCB->suspend_flag==1);
  _nrk_wait_for_scheduler ();
  return NRK_OK;
}


/*
 * nrk_wait_until_ticks()
 *
 * This function will wait until a specified number of
 * timer ticks starting from when the task was swapped in.
 * This means that this function can set periodic timing
 * taking into account any task processing time.
 *
 */

int8_t nrk_wait_until_ticks (uint16_t ticks)
{
  uint8_t timer;
  nrk_int_disable ();
  nrk_cur_task_TCB->suspend_flag = 1;
  nrk_cur_task_TCB->next_wakeup = ticks;
  timer = _nrk_os_timer_get ();

  if (timer < MAX_SCHED_WAKEUP_TIME - TIME_PAD)
    if ((timer + TIME_PAD) <= _nrk_get_next_wakeup ()) {
      timer += TIME_PAD;
      _nrk_prev_timer_val = timer;
      _nrk_set_next_wakeup (timer);
    }
//else nrk_cur_task_TCB->next_wakeup=ticks+1;
  nrk_int_enable ();
//while(nrk_cur_task_TCB->suspend_flag==1);
  _nrk_wait_for_scheduler ();
  return NRK_OK;
}

int8_t nrk_set_next_wakeup (nrk_time_t t)
{
  uint8_t timer;
  uint16_t nw;
  nrk_int_disable ();
  timer = _nrk_os_timer_get ();
  nw = _nrk_time_to_ticks (t);
  if (nw <= TIME_PAD)
    return NRK_ERROR;
  nrk_cur_task_TCB->next_wakeup = nw + timer;
/*	if(timer<(254-TIME_PAD))
		if((timer+TIME_PAD)<=_nrk_get_next_wakeup())
		{
			timer+=TIME_PAD;
			_nrk_prev_timer_val=timer;
			_nrk_set_next_wakeup(timer);
		}
*/
//      nrk_cur_task_TCB->nw_flag=1;
  nrk_int_enable ();

  return NRK_OK;
}

/*
 * nrk_wait_until_nw()
 *
 * This function will wait until a specified number of
 * timer ticks starting from when the task was swapped in.
 * This means that this function can set periodic timing
 * taking into account any task processing time.
 *
 */

int8_t nrk_wait_until_nw ()
{
  uint8_t timer;
  nrk_int_disable ();
  nrk_cur_task_TCB->suspend_flag = 1;
  nrk_cur_task_TCB->nw_flag = 1;
  timer = _nrk_os_timer_get ();

  if (timer < MAX_SCHED_WAKEUP_TIME - TIME_PAD)
    if ((timer + TIME_PAD) <= _nrk_get_next_wakeup ()) {
      timer += TIME_PAD;
      _nrk_prev_timer_val = timer;
      _nrk_set_next_wakeup (timer);
    }
//else nrk_cur_task_TCB->next_wakeup=ticks+1;
  nrk_int_enable ();
//while(nrk_cur_task_TCB->suspend_flag==1);
  _nrk_wait_for_scheduler ();
  return NRK_OK;
}


int8_t nrk_wait (nrk_time_t t)
{
  uint8_t timer;
  uint16_t nw;

  nrk_stack_check ();

  nrk_int_disable ();
  nrk_cur_task_TCB->suspend_flag = 1;
  nrk_cur_task_TCB->num_periods = 1;
  timer = _nrk_os_timer_get ();

//printf( "t1 %lu %lu\n",t.secs, t.nano_secs/NANOS_PER_MS);
  nw = _nrk_time_to_ticks (t);
//printf( "t2 %u\n",nw );
  nrk_cur_task_TCB->next_wakeup = nw + timer;
//printf( "wu %u\n",nrk_cur_task_TCB->next_wakeup );
  if (timer < (MAX_SCHED_WAKEUP_TIME - TIME_PAD))
    if ((timer + TIME_PAD) <= _nrk_get_next_wakeup ()) {
      timer += TIME_PAD;
      _nrk_prev_timer_val = timer;
      _nrk_set_next_wakeup (timer);
    }
  nrk_int_enable ();

  _nrk_wait_for_scheduler ();
  return NRK_OK;
}


inline void _nrk_wait_for_scheduler ()
{

  //TIMSK = BM (OCIE1A);
  do {
    nrk_idle ();                // wait for the interrupt to tick...
  } while ((volatile uint8_t) nrk_cur_task_TCB->suspend_flag == 1);

  //TIMSK = BM (OCIE1A) | BM(OCIE0);
}


int8_t nrk_wait_until (nrk_time_t t)
{
  nrk_time_t ct;
  uint8_t v;

  nrk_time_get (&ct);

  v = nrk_time_sub (&t, t, ct);
  if (v == 0)
    return NRK_ERROR;
//if(t.secs<ct.secs) return 0;
//if(t.secs==ct.secs && t.nano_secs<ct.nano_secs) return 0;

//t.secs-=ct.secs;
//t.nano_secs-=ct.nano_secs;
  nrk_wait (t);

  return NRK_OK;
}


uint8_t nrk_get_pid ()
{
  return nrk_cur_task_TCB->task_ID;
}
